/*
 * @Description: 定义Extract为提取到模型参数，点云不变；Segment为点云分割为不同部分，更改了点云类型
 			Filter为点云去掉一部分，点云量减少
 * @Author: Sang Hao
 * @Date: 2021-11-09 09:57:01
 * @LastEditTime: 2021-11-14 19:43:21
 * @LastEditors: Sang Hao
 */

#ifndef LIDAR_SLAM_MODELS_EXTRACT_PLANE_RANSAC_EXTRACT_HPP_
#define LIDAR_SLAM_MODELS_EXTRACT_PLANE_RANSAC_EXTRACT_HPP_


#include <yaml-cpp/yaml.h>
#include <Eigen/Dense>

#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/method_types.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/normal_space.h>
#include <pcl/filters/extract_indices.h>
#include <boost/thread/thread.hpp>						// ???
#include <pcl/ModelCoefficients.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "lidar_slam/sensor_data/cloud_data.hpp"

// #include <pcl/console/parse.h>




namespace lidar_slam {
class PlaneRansacExtract {
private:
	/* data */
	/* 定义为pcl::SACSegmentation类，就不用计算法线了 */
	/*  */
	pcl::SACSegmentation<CloudData::POINT> sac_segment_;
	// pcl::SACSegmentationFromNormals<CloudData::POINT, pcl::Normal> sac_seg_from_normal_;
	// pcl::ModelCoefficients model_coeff_;
	// pcl::PointIndices inliers_indices_;

	/* Param */
	/* 参数有点多，构造函数给这么多参数，不太好，就这还没有加上模型的参数 */
	// int k_search_ = 10;
	int max_iters_ = 30;
	// float normal_weight_ = 0.1;
	float distance_thresh_ = 0.1;
	float eps_angle_ = 10; /* degree */
	std::string model_type_ = "";
	Eigen::Vector3f axis_ = {0, 0, 1};
	int plane_num_ = 0;
	/* 定义一个分割的flag，如果为真，则将该平面剔除，否则只获得平面参数 */
	bool filter_flag_ = false;

private:

public:
	/* 只保留node构造，因为参数太多了 */
	PlaneRansacExtract(const YAML::Node& config_node);

	std::string GetModelType();
	/* 得到模型参数和模型内点索引 */
	bool Extract(const CloudData::CLOUD_PTR& input_cloud_ptr, 
				 pcl::ModelCoefficients::Ptr& model_coeff_ptr, 
				 pcl::PointIndices::Ptr& inliers_indices_ptr);
	inline int GetPlaneNum() { return plane_num_; }
};


}

#endif